#include "LocationSoftwareOperation.h"
#include <bosch_locator_bridge/ClientSetSeed.h>
#include "std_srvs/Empty.h"
#include <bosch_locator_bridge/StartRecording.h>
#include <bosch_locator_bridge/AddLandMark.h>
#include <bosch_locator_bridge/ClientMapStart.h>
#include <bosch_locator_bridge/StartVisualRecording.h>
#include <bosch_locator_bridge/ServerMapGetImageWithResolution.h>
#include "MapManager.h"
#include <bosch_locator_bridge/ClientMapSet.h>
#include <bosch_locator_bridge/ServerMapList.h>

 namespace kybot_map {

#define CALLLOCATIONSERVER(server)  \
    if (ros::service::call(server, msg)) {\
        fail_reason_ = msg.response.failReason;\
        return true;\
    } else {\
        fail_reason_ = "Call Server Failed";\
        return false;\
    }

    LocationSoftwareOperation::LocationSoftwareOperation()
    {

    }

    bool LocationSoftwareOperation::RelocatorPos(geometry_msgs::Pose2D pose)
    {
        bosch_locator_bridge::ClientSetSeed msg;
        msg.request.x = pose.x;
        msg.request.y = pose.y;
        msg.request.theta = pose.theta;
        LOG(INFO)<<"call ClientSetSeed";
        CALLLOCATIONSERVER("/bridge_node/cliend_set_seed")
    }


    bool LocationSoftwareOperation::StartLocalization(const std::string& param)
    {
        std_srvs::Empty msg;
        if (ros::service::call("/bridge_node/start_localization", msg))
        {
            LOG(INFO)<<"call bridge_node start_localization succeed";
            return true;
        }
        return false;
    }

    bool LocationSoftwareOperation::StopLocalization(const std::string& param)
    {
        std_srvs::Empty msg;
        if (ros::service::call("/bridge_node/stop_localization", msg))
        {
            LOG(INFO)<<"call bridge_node stop_localization succeed";
            return true;
        }
        else
           return false;
    }


    bool LocationSoftwareOperation::SwitchMap(const std::string& map_name)
    {
        bosch_locator_bridge::ClientMapSet msg;
        std_srvs::Empty msg1;
        msg.request.name = map_name;
        if(ros::service::call("/bridge_node/stop_localization", msg1))
        {
            if(ros::service::call("/bridge_node/set_map", msg))
            {  
                if(ros::service::call("/bridge_node/start_localization",msg1))
                {
                    return true;
                }
                else
                {
                    ROS_INFO("start_localization error");
                    return false;
                }
            }
            else
            {
                ROS_INFO("set_map error");
                return false;
            }
        }
        else
        {
           ROS_INFO("stop_localization error");
           return false;
        }
        return true;

    }
    bool LocationSoftwareOperation::SearchMap(std::vector<std::string>& mapNameList_,const std::string& mapFolderPath_)
    {
        mapNameList_.clear();
        bosch_locator_bridge::ServerMapList msg;
        if(ros::service::call("/server_bridge_node/list_server_maps", msg))
        {   
            mapNameList_ = msg.response.names;
            return true;
        }
        else
        {
            ROS_INFO("can not get mapNameList_");
            return false;
        }
        return true;
    }
}